#include "loitordriver.h"
namespace loitor {
intergrate_img::intergrate_img(ros::NodeHandle n):
  it(new image_transport::ImageTransport(n))\
  ,Info_manager(new camera_info_manager::CameraInfoManager(n))\
  ,info_ptr(new sensor_msgs::CameraInfo(Info_manager->getCameraInfo()))
{
  new_image(visensor_resolution_status);

}
int  intergrate_img::load_cinfo(string info_url)
{
 cout<<"loading"<<endl;
//if (Info_manager->validateURL("package://loitor_ros/calibrations/left_camera.yaml"))
//{
 Info_manager->loadCameraInfo("package://loitor_ros/calibrations/left_camera.yaml");
// return 0;
//}
//else
 return -1;
}
void intergrate_img::new_image(bool resolution)
{
  // 创建用来接收camera数据的图像
  /*
  * resolution:  camera当前分辨率状态
  * 0-代表VGA
  * 1-代表WVGA
  */
  cv::Mat img;
  switch (resolution) {
  case 0:
    img.create(cv::Size(640,480),CV_8U);
    img.data=new unsigned char[IMG_WIDTH_VGA*IMG_HEIGHT_VGA];
    break;
  case 1:
    img.create(cv::Size(752,480),CV_8U);
    img.data=new unsigned char[IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA];
    break;
  default:
    cout <<"new image: paramater erro ";
  //  return 0;
    break;
  }
}
void intergrate_img::wrap_msgs()
{
  // 加时间戳
  ros::Time msg_time;

  cv_bridge::CvImage temp;
  temp=cv_bridge::CvImage(std_msgs::Header(), "mono8", img);
  msg_time.sec=time_stamp.tv_sec;
  msg_time.nsec=1000*time_stamp.tv_usec;
  temp.header.stamp = msg_time;
  temp.header.seq=0;
  msg = temp.toImageMsg();

  //FrameID
  std_msgs::Header header;
  header.stamp = msg_time;
  header.frame_id = "camera";
  info_ptr->header = header;
  cout<<"wrap msgs"<<endl;


}
void intergrate_img::set_img(cv::Mat img)
{
  this->img = img;
}
void intergrate_img::set_stamp(timeval stamp)
{
  this->time_stamp = stamp;
}
void intergrate_img::img_pub(string C)
{
 pub = it->advertiseCamera("/camera"+C+"/image_raw", 1);
}

loitordriver::loitordriver(ros::NodeHandle nh):
  n(nh)
{
  imu_start_transfer=true;
  intergrate_img img_temp(n);
  img.clear();
  vector<string> yaml_url={"package://loitor_ros/calibrations/left_camera.yaml","package://loitor_ros/calibrations/right_camera.yaml"};
  string left_info_url("package://loitor_ros/calibrations/left_camera.yaml");
  for(CAMERA i:{LEFT,RIGHT})
  {
    img.push_back(make_shared<intergrate_img>(img_temp));
  }
  cout<<"size:"<<img.size()<<endl;
  cout<<"out:"<<img[LEFT]->load_cinfo(left_info_url)<<endl;
  cout<<"out0:"<<img[RIGHT]->load_cinfo(left_info_url)<<endl;
  run();
  // imu publisher
  pub_imu = n.advertise<sensor_msgs::Imu>("imu0", 200);
  fill_image(visensor_cam_selection);
  Pub_stereoimage();
  //  pub = it->advertiseCamera("/camera/left/image_raw", 1);
//  pub1 = it1->advertiseCamera("/camera/right/image_raw", 1);

}
loitordriver::~loitordriver()
{
  /* shut-down viewers */
  visensor_Close_IMU_viewer=true;
  if(imu_data_thread !=0)
  {
    pthread_join(imu_data_thread,NULL);
  }

  cout<<endl<<"shutting-down Cameras"<<endl;
  /* close cameras */
  visensor_Close_Cameras();
  /* close IMU */
  visensor_Close_IMU();
}
void loitordriver::run()
{
  //start camera
  visensor_Start_Cameras();   //try exception
  // Start IMU
  visensor_Start_IMU();
  usleep(100000);
  //Create imu_data_stream thread
  pthread_create(&imu_data_thread, NULL,&(loitordriver::imu_data_stream),this );
  cout<<"run..."<<endl;
}
void* loitordriver::imu_data_stream(void * arg)
{
  loitordriver* tf_args = static_cast<loitordriver*>(arg) ;
  int counter=0;
  tf_args->imu_start_transfer=false;
  while((!tf_args->visensor_Close_IMU_viewer)&&!tf_args->imu_start_transfer) usleep(1000);
  while(! tf_args->visensor_Close_IMU_viewer)
  {
    if(visensor_imu_have_fresh_data())
            {
      counter++;
      // 每隔20帧显示一次imu数据
      if(counter>=20)
      {
        //cout<<"visensor_imudata_pack->a : "<<visensor_imudata_pack.ax<<" , "<<visensor_imudata_pack.ay<<" , "<<visensor_imudata_pack.az<<endl;
        float ax=visensor_imudata_pack.ax;
        float ay=visensor_imudata_pack.ay;
        float az=visensor_imudata_pack.az;
        //cout<<"visensor_imudata_pack->a : "<<sqrt(ax*ax+ay*ay+az*az)<<endl;
        //cout<<"imu_time : "<<visensor_imudata_pack.imu_time<<endl;
        //cout<<"imu_time : "<<visensor_imudata_pack.system_time.tv_usec<<endl;
        counter=0;
      }
      sensor_msgs::Imu imu_msg;
      imu_msg.header.frame_id = "/imu";
      ros::Time imu_time;
      imu_time.sec=visensor_imudata_pack.system_time.tv_sec;
      imu_time.nsec=1000*visensor_imudata_pack.system_time.tv_usec;
      imu_msg.header.stamp = imu_time;
      imu_msg.header.seq=0;

      imu_msg.linear_acceleration.x=visensor_imudata_pack.ax;
      imu_msg.linear_acceleration.y=visensor_imudata_pack.ay;
      imu_msg.linear_acceleration.z=visensor_imudata_pack.az;
      imu_msg.angular_velocity.x=3.1415926f*visensor_imudata_pack.rx/180.0f;
      imu_msg.angular_velocity.y=3.1415926f*visensor_imudata_pack.ry/180.0f;
      imu_msg.angular_velocity.z=3.1415926f*visensor_imudata_pack.rz/180.0f;
      imu_msg.orientation.w=visensor_imudata_pack.qw;
      imu_msg.orientation.x=visensor_imudata_pack.qx;
      imu_msg.orientation.y=visensor_imudata_pack.qy;
      imu_msg.orientation.z=visensor_imudata_pack.qz;

      tf_args->pub_imu.publish(imu_msg);
    }
    usleep(10);
  }
  pthread_exit(NULL);
}
void loitordriver::Pub_leftimage()
{
   img[LEFT]->img_pub("/left");
}
void loitordriver::Pub_rightimage()
{
  // 显示同步数据的时间戳（单位微秒）
 // cout<<"right_time : "<<right_stamp.tv_usec<<endl;
//  cout<<"paired_imu time ===== "<<img[0].system_time.tv_usec<<endl<<endl;

  img[RIGHT]->img_pub("/right");

}
void loitordriver::Pub_stereoimage()
{
  // 显示同步数据的时间戳（单位微秒）
  //cout<<"left_time : "<<left_stamp.tv_usec<<endl;
  //cout<<"right_time : "<<right_stamp.tv_usec<<endl;
  //cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
  //cout<<"visensor_get_hardware_fps() ==== "<<1.0f/visensor_get_hardware_fps()<<endl;

img[LEFT]->img_pub("/left");
img[RIGHT]->img_pub("/right");
//  pub.publish(msg,lci);
//  pub1.publish(msg1,rci);
cout<<"pub"<<endl;
}
void loitordriver::fill_image (int selection)    //visensor_cam_selection
{
  cout<<"fill..."<<endl;
  cv::Mat img_[2];
  timeval stamp_[2];
 // intergrate_img img_temp(n);
  selection = 0;
  switch (selection) {
  case 0:     //stereo
    paired_imu=visensor_get_stereoImg((char *)img_[LEFT].data,(char *)img_[RIGHT].data,stamp_[LEFT],stamp_[RIGHT]);
    break;
  case 1:     //right
    camera = RIGHT;
    paired_imu=visensor_get_rightImg((char *)img_[RIGHT].data,stamp_[RIGHT]);
    break;
  case 2:     //left
    camera = LEFT;
    paired_imu=visensor_get_leftImg((char *)img_[LEFT].data,stamp_[LEFT]);
    break;
  default:
    cout<<"fill img erro";
    break;
  }
  cout<<"fill ??"<<endl;
  for(CAMERA i:{LEFT,RIGHT})
  {
    img[i]->set_img(img_[i]);
    img[i]->set_stamp(stamp_[i]);
    img[i]->wrap_msgs();
  }
}
}
